///////////////////////////////////////////////////////////////////////////////
//
// Lynx 6 Arm Service - Main Service
//
// Based on code from the Lynxmotion web site arm.1.2.zip
// Modified with the permission of Lynxmotion
//
// Updated Nov-2007 by Trevor Taylor, Software Technology
//
// This software is Open Source
//
//
// Change Log:
//
// Changed the contract identifier to reflect new version and
// also matched the manifest with the id (was wrong).
//
// Renumbered the joints and added the gripper. The joint numbers are
// now in an enum and it *should* be possible to change them, but this
// has not been tested. Need to fix Lynx6ArmConfiguration.cs.
//
// Added some hard-coded limits to joint movements to avoid damage.
// (Look in JointAngleToServoAngle)
//
// Placed time limits on commands to the SSC-32 to limit the speed
// of motions. These times are calculated based on the amount of the
// movement. A new field in the state, MaxAngularSpeed in degrees per
// second. The old speed parameter is still included. You can play
// with MaxAngularSpeed to slow down or speed up the arm. If you make
// it really big, then no time parameters will be sent.
//
// Did some rounding in PulseWidthToAngle and AngleToPulseWidth.
// The problem is that the SSC-32 only returns a single byte for the
// servo positions and loses some precision. The result of using only
// integer arithmetic was that if you positioned the arm then read
// back the pose, you would lose a degree (or two). This has not been
// totally solved, but it is much better.
//
// Removed the Inverse Kinematics service and replaced it with internal
// functions called ForwardKinematics and InverseKinematics. This might
// not have been necessary, but it streamlines the service and I could
// not get the IK service to work properly anyway. I am not convinced
// that the joint parameters in Lynx6ArmConfiguration.cs are correct.
// It does not matter now.
//
// Changed the coordinate system to match the new simulation. If you
// liked the old coordinates, it is a simple matter to transform them.
// The new system is based on looking at the arm from the left-hand side,
// i.e. looking side-on you are facing the switch and serial socket on
// the SSC-32. This is the way the arm usually appears in pictures.
// The X axis is to the right, the Y axis is up and the Z axis points
// towards you making it a right-hand system. Note that this means the
// X values are *always* negative!
// If you only use values returned from GetPose in SetPose calls then
// there will not be any problems.
//
// Created a Conversions class for commonly performed conversions, but
// then got tired of replacing code so there are still some places
// where explicit conversions are performed.
//
// Thought about creating a new generic arm contract so that the gripper
// could be managed separately; multiple joints could be moved; and
// you could specify the motion time. However, it would make the service
// incompatible with existing code, and it was too much trouble. Maybe
// in the next version?
//
// NOTE: Subscriptions have not been tested.
//
// Rebuilt Feb 2008 with a new Contract Id and Namespace for ProMRDS release
//
///////////////////////////////////////////////////////////////////////////////

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
//using wasp = Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.Security.Permissions;
using xml = System.Xml;
using W3C.Soap;
//using soap = W3C.Soap;

using submgr = Microsoft.Dss.Services.SubscriptionManager;
using ssc32 = ProMRDS.Robotics.Lynxmotion.Ssc32.Proxy;
// TT Nov 2007 - Removed the IK service
//using ik = CoroWare.Robotics.Services.Lynx6ArmIK.Proxy;
using armproxy = Microsoft.Robotics.Services.ArticulatedArm.Proxy;
using Microsoft.Robotics.PhysicalModel.Proxy;
using physicalmodel = Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;

namespace ProMRDS.Robotics.Lynxmotion.Lynx6Arm
{
    /// <summary>
    /// Controls a Lynx6 arm using an inverse kinematics partner service for end effector control
    /// </summary>
    [DisplayName("LynxMotion Lynx6 Arm")]
    [Description("Controls a Lynx6 Arm")]
    [AlternateContract(armproxy.Contract.Identifier)]
    [Contract(Contract.Identifier)]
    public class Lynx6ArmService : DsspServiceBase
    {
        //Partners
        [Partner("SSC32", Contract = ssc32.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
        private ssc32.SSC32Operations _ssc32Port = new ssc32.SSC32Operations();

// TT - Removed the IK service
//        [Partner("Lynx6ArmIK", Contract = ik.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
//        private ik.Lynx6ArmIKOperations _ikPort = new ik.Lynx6ArmIKOperations();

        [ServicePort("/Lynx6Arm", AllowMultipleInstances = true)]
        private Lynx6ArmOperations _mainPort = new Lynx6ArmOperations();

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        [AlternateServicePort("/arm", AlternateContract = armproxy.Contract.Identifier)]
        private armproxy.ArticulatedArmOperations _armPort = new armproxy.ArticulatedArmOperations();

        private Lynx6ArmState _state = new Lynx6ArmState();
        private Dictionary<string, Joint> _jointLookup;
        private Dictionary<string, int> _channelLookup;
        private Lynx6ArmConfiguration _armConfig;

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public Lynx6ArmService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {

        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeArmState();

            // Listen on the main port for requests and call the appropriate handler.
            Activate(
                Arbiter.Interleave(
                    new TeardownReceiverGroup(
                        Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler)
                    ),
                    new ExclusiveReceiverGroup
                    (
                        Arbiter.ReceiveWithIterator<armproxy.SetEndEffectorPose>(true, _armPort, SetEndEffectorHandler),
                        Arbiter.ReceiveWithIterator<armproxy.SetJointTargetPose>(true, _armPort, SetJointPoseHandler),
                        Arbiter.ReceiveWithIterator<armproxy.SetJointTargetVelocity>(true, _armPort, SetJointVelocityHandler)
                    ),
                    new ConcurrentReceiverGroup
                    (
                        Arbiter.ReceiveWithIterator<armproxy.Subscribe>(true, _armPort, SubscribeHandler),
                        Arbiter.ReceiveWithIterator<armproxy.GetEndEffectorPose>(true, _armPort, GetEndEffectorHandler),
                        Arbiter.Receive<DsspDefaultLookup>(true, _armPort, DefaultLookupHandler),
                        Arbiter.ReceiveWithIterator<Get>(true, _mainPort, GetHandler),                   
                        // Alternate contract Get (in addition to main Get)
                        Arbiter.ReceiveWithIterator<armproxy.Get>(true, _armPort, ArmGetHandler)
                    ))
            );

            // Publish the service to the local Node Directory
            DirectoryInsert();

			// display HTTP service Uri
			LogInfo(LogGroups.Activation, "Service uri: " + base.FindServiceAliasFromScheme(Uri.UriSchemeHttp));
        }

        private void InitializeArmState()
        {
            if (_state == null)
            {
                _state = new Lynx6ArmState();

            }
            _armConfig = new Lynx6ArmConfiguration();
            _state.Joints = _armConfig.Joints;

            _jointLookup = new Dictionary<string, Joint>();
            _channelLookup = new Dictionary<string, int>();

            for (int i = 0; i < _state.Joints.Count; i++)
            {
                Joint j = _state.Joints[i];
                j.State.Angular.DriveTargetOrientation = new Quaternion(0,0,0,1);

                // TT Nov-2007 -
                // The original comment here was wrong.
                // Joint speeds are confusing. Pulse widths range from 500uS to 2500uS for
                // a range of -90 to 90 degrees. So 1000uS is equivalent to 90 degrees,
                // so a speed of 100uS per second will take 10 seconds to travel 90 degrees.
                //
                // Initialize joint speeds to be 500uS per second (takes 2 seconds to move 90 degrees)
                Vector3 angularVelocity = ProxyConversion.ToProxy(physicalmodel.Vector3.Scale(
                                            ProxyConversion.FromProxy(j.State.Connectors[0].JointAxis), 500f));
                j.State.Angular.DriveTargetVelocity = angularVelocity;

                // create dictionary for quick lookup of joints from name
                _jointLookup.Add(j.State.Name, j);
                _channelLookup.Add(j.State.Name, i);

                /*
                // Take out the channel munging and leave
                if (i == 4)
                {
                    _channelLookup.Add(j.State.Name, i+1);
                }
                else
                {
                    _channelLookup.Add(j.State.Name, i);
                }
                 */

            }

//            SpawnIterator(InitializeIKService);
        }

        /*
        private IEnumerator<ITask> InitializeIKService()
        {
            //initialize the IK service with the lynx 6 arm configuration           
            ik.Lynx6ArmIKState armState = new ik.Lynx6ArmIKState();
            armState.ArmState = (armproxy.ArticulatedArmState) (_state.Clone());

            yield return Arbiter.Choice(
                _ikPort.Replace(armState),
                delegate(DefaultReplaceResponseType response)
                {
                },
                delegate(Fault f)
                {
                    LogError("Lynx6ArmIK service cannot be started.");
                }
            );
        }
        */

        #region Operations Handlers

        /// <summary>
        /// Set End Effector
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        /// <remarks>Moves the arm end effector to a specified pose</remarks>
        private IEnumerator<ITask> SetEndEffectorHandler(armproxy.SetEndEffectorPose update)
        {
            float x, y, z, p;
            float baseAngle = 0, shoulder = 0, elbow = 0, wrist = 0;

            // Get all the pose information required
            // NOTE: The angle is assumed to be about the X axis of the gripper
            physicalmodel.AxisAngle a;
            physicalmodel.Quaternion q = new physicalmodel.Quaternion(
                update.Body.EndEffectorPose.Orientation.X,
                update.Body.EndEffectorPose.Orientation.Y,
                update.Body.EndEffectorPose.Orientation.Z,
                update.Body.EndEffectorPose.Orientation.W);
            a = physicalmodel.Quaternion.ToAxisAngle(q);
            p = Conversions.RadiansToDegrees(a.Angle * Math.Sign(a.Axis.X));
            x = update.Body.EndEffectorPose.Position.X;
            y = update.Body.EndEffectorPose.Position.Y;
            z = update.Body.EndEffectorPose.Position.Z;

            // Use Inverse Kinematics to calculate the joint angles
            if (!InverseKinematics(x, y, z, p, ref baseAngle, ref shoulder, ref elbow, ref wrist))
            {
                // The results were not a valid pose!
                // Don't attempt the move or it might break the arm
                Fault f;
                // Should put something in the Fault!
                f = Fault.FromException(new Exception("No solution to Inverse Kinematics"));
                update.ResponsePort.Post(f);
                yield break;
            }

            // Sanity check the result because the solution might be
            // for an impossible combination of joint angles.
            // Check servo bounds of +/- 90 even though some joints can't move
            // all this way. Be careful!
            // If the target is invalid, post an exception message to the
            // response port with a helpful error.
            if ((baseAngle < -90) || (baseAngle > 90))
            {
                update.ResponsePort.Post(Fault.FromException(
                    new Exception(_state.Joints[(int)JointNumbers.Base].State.Name + " angle invalid: " + baseAngle.ToString())));
                yield break;
            }

            if ((shoulder < -90) || (shoulder > 90))
            {
                update.ResponsePort.Post(Fault.FromException(
                    new Exception(_state.Joints[(int)JointNumbers.Shoulder].State.Name + " angle invalid: " + shoulder.ToString())));
                yield break;
            }

            if ((elbow < -90) || (elbow > 90))
            {
                update.ResponsePort.Post(Fault.FromException(
                    new Exception(_state.Joints[(int)JointNumbers.Elbow].State.Name + " angle invalid: " + elbow.ToString())));
                yield break;
            }

            if ((wrist < -90) || (wrist > 90))
            {
                update.ResponsePort.Post(Fault.FromException(
                    new Exception(_state.Joints[(int)JointNumbers.Wrist].State.Name + " angle invalid: " + wrist.ToString())));
                yield break;
            }

            // Process the new angles
            float angleDiff;
            int moveTime;
            int i = 0;
            int index;
            float temp;
            int[] channels = {
                        (int)JointNumbers.Base,
                        (int)JointNumbers.Shoulder,
                        (int)JointNumbers.Elbow,
                        (int)JointNumbers.Wrist };

            int[] servoAngles = new int[channels.Length];
            int[] pulseWidths = new int[channels.Length];
            int[] speeds = new int[channels.Length];

            // Calculate the difference between the current angle and the
            // requested angle and then figure out the time to take
            // Update the joint angle in the state now as well
            // NOTE: This will also be updated during a Get request
            // with info from the arm itself
            index = (int)JointNumbers.Base;
            angleDiff = Math.Abs(baseAngle - _state.Angles[index]);
            _state.Angles[index] = baseAngle;
            servoAngles[i] = JointAngleToServoAngle(index, (int)Math.Round(baseAngle));
            pulseWidths[i] = AngleToPulseWidth(servoAngles[i]);
            speeds[i] = (int) physicalmodel.Vector3.Length(
                                    ProxyConversion.FromProxy(_state.Joints[index].State.Angular.DriveTargetVelocity));
            i++;

            index = (int)JointNumbers.Shoulder;
            temp = Math.Abs(shoulder - _state.Angles[index]);
            if (temp > angleDiff)
                angleDiff = temp;
            _state.Angles[index] = shoulder;
            servoAngles[i] = JointAngleToServoAngle(index, (int)Math.Round(shoulder));
            pulseWidths[i] = AngleToPulseWidth(servoAngles[i]);
            speeds[i] = (int)physicalmodel.Vector3.Length(
                                    ProxyConversion.FromProxy(_state.Joints[index].State.Angular.DriveTargetVelocity));
            i++;

            index = (int)JointNumbers.Elbow;
            temp = Math.Abs(elbow - _state.Angles[index]);
            if (temp > angleDiff)
                angleDiff = temp;
            _state.Angles[index] = elbow;
            servoAngles[i] = JointAngleToServoAngle(index, (int)Math.Round(elbow));
            pulseWidths[i] = AngleToPulseWidth(servoAngles[i]);
            speeds[i] = (int)physicalmodel.Vector3.Length(
                                    ProxyConversion.FromProxy(_state.Joints[index].State.Angular.DriveTargetVelocity));
            i++;

            index = (int)JointNumbers.Wrist;
            temp = Math.Abs(wrist - _state.Angles[index]);
            if (temp > angleDiff)
                angleDiff = temp;
            _state.Angles[index] = wrist;
            servoAngles[i] = JointAngleToServoAngle(index, (int)Math.Round(wrist));
            pulseWidths[i] = AngleToPulseWidth(servoAngles[i]);
            speeds[i] = (int)physicalmodel.Vector3.Length(
                                    ProxyConversion.FromProxy(_state.Joints[index].State.Angular.DriveTargetVelocity));
            i++;

            // Calculate the elapsed time for the move
            moveTime = (int)((angleDiff / _state.MaxAngularSpeed) * 1000.0f);
            // If the time is very small, ignore it
            if (moveTime < 20)
                moveTime = -1;

            // Now create the request
            ssc32.SSC32ServoMove moveCommand = new ssc32.SSC32ServoMove();
            moveCommand.Channels = channels;
            moveCommand.PulseWidths = pulseWidths;
            moveCommand.Speeds = speeds;
            // Always apply some sort of reasonable time so that the arm does
            // not move suddenly
            moveCommand.Time = moveTime;

            // Execute the command
            ssc32.SendSSC32Command command = new ssc32.SendSSC32Command(moveCommand);
            _ssc32Port.Post(command);
            yield return Arbiter.Choice(command.ResponsePort,
                delegate(ssc32.SSC32ResponseType response)
                {
                    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                }
            );

            // Send a replace notification since end effector pose updates affect all joints
            // NOTE: This needs work! The State is not yet correct.
            armproxy.Replace replace = new armproxy.Replace();
            replace.Body = (armproxy.ArticulatedArmState)_state;
            base.SendNotification(_submgrPort, replace);

            // Send an end effector update notification as well
            base.SendNotification(_submgrPort, update);

            yield break;

            // TT Nov-2007 - Remove IK
            /*
            ik.SetEndEffectorPoseResponse ikResponse = null;
            
            ik.SetEndEffectorPoseRequest request  = new ik.SetEndEffectorPoseRequest();
            request.EndEffectorPose = update.Body.EndEffectorPose;
            yield return Arbiter.Choice(
                _ikPort.SetEndEffectorPose(request),
                delegate(ik.SetEndEffectorPoseResponse response)
                {
                    ikResponse = response;
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                }
            );

            if (ikResponse == null)
                yield break;

            PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();

            for (int i = 0; i < ikResponse.JointNames.Count; i++)
            {
                armproxy.SetJointTargetPoseRequest poseRequest = new armproxy.SetJointTargetPoseRequest();
                poseRequest.JointName = ikResponse.JointNames[i];
                poseRequest.TargetOrientation = ikResponse.JointOrientations[i];
                _armPort.Post(new armproxy.SetJointTargetPose(poseRequest,responsePort));
            }
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield return Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>
                (responsePort, ikResponse.JointNames.Count,
                delegate(ICollection<DefaultUpdateResponseType> successes, ICollection<Fault> faults) { });

            // we send a replace notification since end effector pose updates affect all joints
            armproxy.Replace replace = new armproxy.Replace();
            replace.Body = (armproxy.ArticulatedArmState)_state;
            base.SendNotification(_submgrPort, replace);

            // send an end effector update notification as well
            base.SendNotification(_submgrPort, update);
            */
        }

        // This method is executed if the MoveTo method fails
        void ShowError(Exception e)
        {
            Console.WriteLine(e.Message);
        }

        /// <summary>
        /// Get End Effector
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        /// <remarks>Gets the current pose of the arm end effector</remarks>
        private IEnumerator<ITask> GetEndEffectorHandler(armproxy.GetEndEffectorPose get)
        {
            float x = 0, y = 0, z = 0, p = 0;
            float baseAngle = 0, shoulder = 0, elbow = 0, wrist = 0;
            bool ok = false;

            // Get the current joint angles for all joints
            yield return Arbiter.Choice(
                UpdateState(),
                delegate(SuccessResult success)
                {
                    ok = true;
                },
                delegate(Exception ex)
                {
                    get.ResponsePort.Post(Fault.FromException(ex));
                }
            );

            if (!ok)
                yield break;

            baseAngle = _state.Angles[(int)JointNumbers.Base];
            shoulder = _state.Angles[(int)JointNumbers.Shoulder];
            elbow = _state.Angles[(int)JointNumbers.Elbow];
            wrist = _state.Angles[(int)JointNumbers.Wrist];

            ForwardKinematics(baseAngle, shoulder, elbow, wrist, ref x, ref y, ref z, ref p);

            // Create a response
            armproxy.GetEndEffectorPoseResponse response = new armproxy.GetEndEffectorPoseResponse();
            // What a pain in the neck!!!
            // All because standard classes cannot be assigned to their proxies!
            Pose rp = new Pose();
            Vector3 v = new Vector3(x, y, z);
            rp.Position = v;
            physicalmodel.Quaternion q = new physicalmodel.Quaternion();
            q = physicalmodel.Quaternion.FromAxisAngle(0, 0, 1, p);
            Quaternion qp = new Quaternion(q.X, q.Y, q.Z, q.W);
            rp.Orientation = qp;
            response.EndEffectorPose = rp;

            // Send back the response
            get.ResponsePort.Post(response);

            yield break;

            // TT Nov-2007 - Remove IK
            /*
            Boolean hasError = true;

            Get getRequest = new Get();
            _mainPort.Post(getRequest);

            yield return Arbiter.Choice(getRequest.ResponsePort,
                delegate(Lynx6ArmState state)
                {
                    hasError = false;
                },
                delegate(Fault fault)
                {
                    get.ResponsePort.Post(fault);
                }
            );

            if (hasError)
                yield break;
            
            ik.GetEndEffectorPoseRequest ikRequest = new ik.GetEndEffectorPoseRequest();
            ikRequest.JointNames = new List<string>();
            ikRequest.JointOrientations = new List<AxisAngle>();
            foreach (Joint j in _state.Joints)
            {
                ikRequest.JointNames.Add(j.State.Name);
                AxisAngle currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                            ProxyConversion.FromProxy(j.State.Angular.DriveTargetOrientation)));
                ikRequest.JointOrientations.Add(currentAngle);
            }
            yield return Arbiter.Choice(
                _ikPort.GetEndEffectorPose(ikRequest),
                delegate(ik.GetEndEffectorPoseResponse ikResponse)
                {
                    armproxy.GetEndEffectorPoseResponse response = new armproxy.GetEndEffectorPoseResponse();
                    response.EndEffectorPose = ikResponse.EndEffectorPose;
                    get.ResponsePort.Post(response);
                },
                delegate(Fault fault)
                {
                    get.ResponsePort.Post(fault);
                }
            );
             */
        }
        
        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        /// <remarks>Get the state for the service</remarks>
        private IEnumerator<ITask> GetHandler(Get get)
        {
            yield return Arbiter.Choice(
                UpdateState(),
                delegate(SuccessResult success)
                {
                    get.ResponsePort.Post(_state);
                },
                delegate(Exception ex)
                {
                    get.ResponsePort.Post(Fault.FromException(ex));
                }
            );
        }

        public IEnumerator<ITask> SubscribeHandler(armproxy.Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;
            SubscribeHelper(_submgrPort, request, subscribe.ResponsePort);
            yield break;
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        /// <remarks>Get handler for Generic Arm contract</remarks>
        private IEnumerator<ITask> ArmGetHandler(armproxy.Get get)
        {
            yield return Arbiter.Choice(
                UpdateState(),
                delegate(SuccessResult success)
                {
                    get.ResponsePort.Post((armproxy.ArticulatedArmState) (_state.Clone()));
                },
                delegate(Exception ex)
                {
                    get.ResponsePort.Post(Fault.FromException(ex));
                }
            );
        }

        // Synchronize the state by reading the joint positions from the controller
        private SuccessFailurePort UpdateState()
        {
            SuccessFailurePort resultPort = new SuccessFailurePort();

            //Create new query pulse width command
            int[] channels = new int[Lynx6ArmState.NUM_JOINTS] { 0, 1, 2, 3, 4, 5 };
            ssc32.SSC32QueryPulseWidth queryCommand = new ssc32.SSC32QueryPulseWidth();
            queryCommand.Channels = channels;

            ssc32.SendSSC32Command command = new ssc32.SendSSC32Command(queryCommand);
            _ssc32Port.Post(command);

            //Update the arm state based on the query response
            Activate(Arbiter.Choice(command.ResponsePort,
                delegate(ssc32.SSC32ResponseType response)
                {
                    ssc32.SSC32PulseWidthResponse queryResponse = (ssc32.SSC32PulseWidthResponse)response;
                    for (int i = 0; i < _state.Joints.Count; i++)
                    {
                        int jointAngle = ServoAngleToJointAngle(i, PulseWidthToAngle(queryResponse.PulseWidths[i]));
                        _state.Joints[i].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(jointAngle);
                        // Remember the angle too - much easier to use!
                        _state.Angles[i] = jointAngle;
                    }
                    // TT Nov-2007 - Note here that the Gripper was 4 and the Wrist Rotate was 5
                    // As per the documentation with the Lynx6Arm service. However, this has been
                    // changed to be in line with the simulation.
                    //int jointAngle4 = ServoAngleToJointAngle(4, PulseWidthToAngle(queryResponse.PulseWidths[5]));
                    //_state.Joints[4].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(jointAngle4);
                    //_state.GripperAngle = PulseWidthToAngle(queryResponse.PulseWidths[4]);
                    _state.GripperAngle = (int)Math.Round(_state.Angles[(int)JointNumbers.Gripper]);

                    resultPort.Post(new SuccessResult());
                },
                delegate(Fault fault)
                {
                    resultPort.Post(new Exception(fault.Reason[0].ToString()));
                }
            ));

            return resultPort;
        }

        /// <summary>
        /// Set Joint Pose
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        /// <remarks>Sets the pose of a single joint (in most cases this is just an angle)</remarks>
        private IEnumerator<ITask> SetJointPoseHandler(armproxy.SetJointTargetPose update)
        {
            // Find out which joint it is from the name
            string name = update.Body.JointName;
            int index = _channelLookup[name];

            // Get the joint
            // Throws an exception if the name does not exist
            Joint j = _jointLookup[name];

            // Figure out the desired angle in degrees
            AxisAngle orientation = update.Body.TargetOrientation;
            float orientationAngle = orientation.Angle * Math.Sign(orientation.Axis.X);
            float jointAngle = Conversions.RadiansToDegrees(orientationAngle);

            float angleDiff;
            int moveTime;

            // Calculate the difference between the current angle and the
            // requested angle and then figure out the time to take
            angleDiff = Math.Abs(jointAngle - _state.Angles[index]);
            // Calculate the elapsed time for the move
            moveTime = (int)((angleDiff / _state.MaxAngularSpeed) * 1000.0f);
            // If the time is very small, ignore it
            if (moveTime < 20)
                moveTime = -1;

            // For debugging ONLY
            //LogInfo(LogGroups.Console, "Set Joint: " + name + "," +
            //            jointAngle + "," + angleDiff + "," +
            //            moveTime);

            // Update the joint angle in the state now
            // NOTE: This will also be updated during a Get request
            // with info from the arm itself
            _state.Angles[index] = jointAngle;
            if (index == (int)JointNumbers.Gripper)
                _state.GripperAngle = (int)Math.Round(jointAngle);

            int servoAngle = JointAngleToServoAngle(index, (int)(orientationAngle * 180 / Math.PI));
            int pulseWidth = AngleToPulseWidth(servoAngle);

            ssc32.SSC32ServoMove moveCommand = new ssc32.SSC32ServoMove();
            moveCommand.Channels = new int[1] { index };
            moveCommand.PulseWidths = new int[1] { pulseWidth };
            moveCommand.Speeds = new int[1] {(int) physicalmodel.Vector3.Length(
                                                    ProxyConversion.FromProxy(j.State.Angular.DriveTargetVelocity))};
            // Always apply some sort of reasonable time so that the arm does
            // not move suddenly
            moveCommand.Time = moveTime;

            ssc32.SendSSC32Command command = new ssc32.SendSSC32Command(moveCommand);
            _ssc32Port.Post(command);
            yield return Arbiter.Choice(command.ResponsePort,
                delegate(ssc32.SSC32ResponseType response)
                {
                    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                }
            );
        }

        /// <summary>
        /// Set Joint Velocity
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        private IEnumerator<ITask> SetJointVelocityHandler(armproxy.SetJointTargetVelocity update)
        {
            Joint j = _jointLookup[update.Body.JointName];
            j.State.Angular.DriveTargetVelocity = update.Body.TargetVelocity;            
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        #endregion

        #region Utility Functions

        // Physical attributes of the arm
        float L1 = Conversions.InchesToMeters(4.75f);   // Length of upper arm
        float L2 = Conversions.InchesToMeters(4.75f);   // Length of lower arm

        // Not required
        //float Grip = Conversions.InchesToMeters(3.5f);  // Length of gripper

        // NOTE: The gripper is basically in two parts because it contains
        // the wrist rotate joint which is NOT at the same place as the
        // wrist joint.
        // Measurements are:
        // Wrist joint to rotate plate = 2.25"
        // Rotate place to tip of fingers = 3.5"
        // The actual length of the "hand" varies slightly depending on
        // whether the gripper is open or closed.
        // L3 measures the distance to the tip of the fingers or End Of Arm (EOA).
        // A Tool Center Point (TCP) would be shorter than this.
        // The rubber fingers are just over 1" long, so to keep the grip action
        // happening on the fingers, then L3 needs to be shorter by about 0.5".
        float L3 = Conversions.InchesToMeters(5.75f);   // Length of whole hand

        float H = Conversions.InchesToMeters(2.5f);     // Height of the base
        // Not required
        //float G = Conversions.InchesToMeters(1.75f);    // Radius of the base

        // Perform the Inverse Kinematics calculations
        //
        bool InverseKinematics(
            float mx, // x position
            float my, // y position
            float mz, // z position
            float p,  // angle of the grip
            ref float baseAngle,
            ref float shoulder,
            ref float elbow,
            ref float wrist)
        {
            // Algorithm taken from Hoon Hong's ik2.xls IK method posted on the Lynx website

            float r = (float)Math.Sqrt(mx * mx + mz * mz); // horizontal distance to target
            baseAngle = (float)Math.Atan2(-mz, -mx); // angle to the target

            float pRad = Conversions.DegreesToRadians(p);
            float rb = (float)((r - L3 * Math.Cos(pRad)) / (2 * L1));
            float yb = (float)((my - H - L3 * Math.Sin(pRad)) / (2 * L1));
            float q = (float)(Math.Sqrt(1 / (rb * rb + yb * yb) - 1));
            float p1 = (float)(Math.Atan2(yb + q * rb, rb - q * yb));
            float p2 = (float)(Math.Atan2(yb - q * rb, rb + q * yb));
            shoulder = p1 - Conversions.DegreesToRadians(90);  // angle of the shoulder joint
            elbow = p2 - shoulder;                 // angle of the elbow joint
            wrist = pRad - p2;                     // angle of the wrist joint

            // Convert all values to degrees
            baseAngle = Conversions.RadiansToDegrees(baseAngle);
            shoulder = Conversions.RadiansToDegrees(shoulder);
            elbow = Conversions.RadiansToDegrees(elbow);
            wrist = Conversions.RadiansToDegrees(wrist);
            // Check to make sure that the solution is valid
            if (Single.IsNaN(baseAngle) ||
                Single.IsNaN(shoulder) ||
                Single.IsNaN(elbow) ||
                Single.IsNaN(wrist))
            {
                // For debugging
                Console.WriteLine("No solution to Inverse Kinematics");
                return false;
            }
            if ((baseAngle > 90) || (baseAngle < -90) ||
                (shoulder > 90) || (shoulder < -90) ||
                (elbow > 90) || (elbow < -90) ||
                (wrist > 90) || (wrist < -90))
            {
                // For debugging
                Console.WriteLine("Invalid angle from Inverse Kinematics");
                return false;
            }
            return true;
        }

        void ForwardKinematics(
            float baseAngle,
            float shoulder,
            float elbow,
            float wrist,
            ref float mx, // x position
            ref float my, // y position
            ref float mz, // z position
            ref float p)  // angle of the grip
        {
            float shoulderAngle, elbowAngle, wristAngle;
            float height, depth;

            // Accumulate the angles along the links
            shoulderAngle = Conversions.DegreesToRadians(90 - shoulder);
            elbowAngle = Conversions.DegreesToRadians(90 - elbow) + shoulderAngle;
            wristAngle = Conversions.DegreesToRadians(-wrist) + elbowAngle;
            // Sum up the height and depth for all links
            height = H;
            depth = 0;
            // Upper Arm L1
            height += (float)(L1 * Math.Sin(shoulderAngle));
            depth += (float)(L1 * Math.Cos(shoulderAngle));
            // Lower Arm L2
            height += (float)(L2 * Math.Sin(elbowAngle));
            depth += (float)(L2 * Math.Cos(elbowAngle));
            // Hand L3
            height += (float)(L3 * Math.Sin(wristAngle));
            depth += (float)(L3 * Math.Cos(wristAngle));

            // Calculate the 3D coordinates
            mx = (float)(-depth * Math.Sin(Conversions.DegreesToRadians(baseAngle - 90)));
            my = height;
            mz = (float)(depth * Math.Cos(Conversions.DegreesToRadians(baseAngle - 90)));
            // Now calculate the gripper angle relative to horizontal
            p = Conversions.RadiansToDegrees(wristAngle);
            // Flip the orientation because X is always negative
            p = 180 - p;
            // Force to the range +/-180
            if (p > 180)
                p = p - 360;
            if (p < -180)
                p = p + 360;
        }
        

        //Convert pulse width to degree angle
        private int PulseWidthToAngle(int pulsewidth)
        {
            if (pulsewidth > 2500)
            {
                pulsewidth = 2500;                
                LogError("Pulse Width out of range.");                
            }
            else if (pulsewidth < 500)
            {
                pulsewidth = 500;
                LogError("Pulse Width out of range.");
            }

            // TT Nov-2007 - Try to make this closer to the actual angle
            int angle = (int)Math.Round(((float)pulsewidth - 1500.0f) * 180.0f / 2000.0f);
            return angle;
        }

        //Convert degree to pulse width
        private int AngleToPulseWidth(int angle)
        {
            if (angle > 90)
            {
                angle = 90;
                LogError("Angle out of range.");
            }
            else if (angle < -90)
            {
                angle = -90;
                LogError("Angle out of range.");
            }

            // TT Nov-2007 - Try to make this closer to the actual pulsewidth
            int pulsewidth = (int)Math.Round((float)angle * 2000.0f / 180.0f + 1500.0f);
            return pulsewidth;
        }

        //Convert an angle in degrees to an orientation quaternion
        private Quaternion AngleToOrientationQuaternion(int angle)
        {
            physicalmodel.AxisAngle rotate = new physicalmodel.AxisAngle(new physicalmodel.Vector3(1, 0, 0), 
                                                (float)(Math.PI * (double)angle / 180));
            // joint axis is always the positive X axis
            Quaternion q = ProxyConversion.ToProxy(physicalmodel.Quaternion.FromAxisAngle(rotate));
            return q;
        }

        //Convert from the servo angle reading to a LH joint angle
        // TT Nov-2007 - Change the reference frame to match the simulation
        private int ServoAngleToJointAngle(int index, int servoAngle)
        {            
            switch (index)
            {
                case (int)JointNumbers.Base: // Base
                    return servoAngle;

                case (int)JointNumbers.Shoulder: // Shoulder
                    return servoAngle;
//                    return -1 * servoAngle;

                case (int)JointNumbers.Elbow: // Elbow
//                    return servoAngle;
//                    return servoAngle - (int)(Lynx6ArmConfiguration.ANGLE2_OFFSET * 180 / Math.PI);
                    return -1 * servoAngle + (int)(Lynx6ArmConfiguration.ANGLE2_OFFSET * 180 / Math.PI);

                case (int)JointNumbers.Wrist: // Wrist
                    return servoAngle;
//                    return -1 * servoAngle;

                case (int)JointNumbers.WristRotate: // Wrist Rotate
                    return servoAngle;

                case (int)JointNumbers.Gripper: // TT - Added gripper
                    return servoAngle;
            }
            return -1;
        }

        //Convert from the LH joint angle to a servo reading
        // TT Nov-2007 -
        // NOTE: For the safety of the arm, some fixed constraints are
        // applied here. We don't want to break the arm!
        // These values are hard-coded. They should probably be in the
        // state, but they are unlikey to change and then they would be
        // open to tampering.
        private int JointAngleToServoAngle(int index, int jointAngle)
        {
            string jointName = _state.Joints[index].State.Name;
            switch (index)
            {
                case (int)JointNumbers.Base: // Base
                    if (jointAngle < -90)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = -90;
                    }
                    if (jointAngle > 90)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = 90;
                    }
                    return jointAngle;

                case (int)JointNumbers.Shoulder: // Shoulder
                    if (jointAngle < -90)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = -90;
                    }
                    if (jointAngle > 90)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = 90;
                    }
                    return jointAngle;
//                    return -1 * jointAngle;

                case (int)JointNumbers.Elbow: // Elbow
//                    return jointAngle;
//                    return (int)(Lynx6ArmConfiguration.ANGLE2_OFFSET * 180 / Math.PI) + jointAngle;
                    jointAngle = -1 * jointAngle + (int)(Lynx6ArmConfiguration.ANGLE2_OFFSET * 180 / Math.PI);
                    if (jointAngle < -90)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = -90;
                    }
                    if (jointAngle > 90)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = 90;
                    }
                    return jointAngle;

                case (int)JointNumbers.Wrist: // Wrist
                    if (jointAngle < -80)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = -80;
                    }
                    if (jointAngle > 80)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = 80;
                    }
                    return jointAngle;
//                    return -1 * jointAngle;

                case (int)JointNumbers.WristRotate: // Wrist Rotate
                    if (jointAngle < -70)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = -70;
                    }
                    if (jointAngle > 70)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = 70;
                    }
                    return jointAngle;

                case (int)JointNumbers.Gripper: // TT - Add gripper
                    if (jointAngle < -35)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = -35;
                    }
                    if (jointAngle > 35)
                    {
                        Console.WriteLine(jointName + " angle set to invalid value: " + jointAngle);
                        jointAngle = 35;
                    }
                    return jointAngle;
            }
            return -1;
        }

        #endregion

    }

    // Utility methods combined into a single class

    public class Conversions
    {
        public static float DegreesToRadians(float degrees)
        {
            return (float)(degrees * Math.PI / 180);
        }

        public static float RadiansToDegrees(float radians)
        {
            return (float)(radians * 180 / Math.PI);
        }

        public static float InchesToMeters(float inches)
        {
            return (float)(inches * 0.0254);
        }

    }

}
